
#define F_CPU 16000000UL
#include <avr/io.h>
#include <avr/wdt.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <stdbool.h>
#include <util/atomic.h>
#include <inttypes.h>
#include "i2c.h"
#include "servo.h"
#include "a2d.h"

#define IO_PORT_DDR 0x08
#define IO_PORT_VALUE 0x09
#define IO_PORT_MASK 0xFF

#define TWI_SLA       0x15  /**< i2c slave address */

#define CHECK_BIT(var,pos) ((var) & (1<<(pos)))
/**
 * Three bytes have to be received for a full command. This enum is used to
 * indicate what part of the command we are waiting for.
 */
typedef enum {
    WAIT_FOR_ADDRESS,     /**< first byte is the address */
    WAIT_FOR_VALUE,       /**< second byte is the lower part of the value */
} ReadCommandState;

/**
 * Holds one command that is received via i2c. The command consists of an
 * address (number of output channel) and a 16-bit value. The state is used to
 * indicate what part of the next command we are waiting for.
 */
typedef struct {
    uint8_t address;         /**< number of output channel (between 0 and
                               CHANNEL_COUNT-1 */
    uint8_t value;          /**< value to be assigned to the channel (between
                               0 and 128*128-1 = 16383 */
    ReadCommandState state;  /**< what are we waiting for? */

} Command;

/** the next command is built in this variable */
Command command = {0, 0, WAIT_FOR_ADDRESS};

bool commandReceived = false ;

uint8_t adcs [6] = {0, 0, 0, 0, 0, 0};


void init_led(){
	DDRD |= 0x01 ;
	PORTD |= 0x01 ;
}

void toggle_led(){
	PORTD = PORTD ^0x01 ;
}

void evaluate_i2c_input(u08 receiveDataLength, u08* recieveData) {
	int i ;
	for(i = 0 ; i < receiveDataLength ; i ++ ){
        // we have input
        u08 byte_received = recieveData[i];
        switch(command.state){
            case WAIT_FOR_ADDRESS:
                if (byte_received & 0x80) {
                    // bit 7 is set -> address received
                    command.address = (byte_received & 0x7F);
                    command.state = WAIT_FOR_VALUE;
                }
                // do nothing if this byte didn't look like an address
                break;
			case WAIT_FOR_VALUE:
                command.value = byte_received ;
				command.state = WAIT_FOR_ADDRESS;
				commandReceived = true ;
                break;
        }
	}
}


u08 read_callback(u08 transmitDataLengthMax, u08* transmitData){
    switch(command.state){
		case WAIT_FOR_VALUE:
			if(command.address < 6){
            	transmitData[0] = servoGetPosition(command.address) ;
			}else if(command.address < 12){
				transmitData[0] = adcs[command.address - 8] ;
			}else if(command.address < 13){
				transmitData[0] = PIND & IO_PORT_MASK ;
			}
			else{
				transmitData[0] =  0x00 ;
			}
			command.state = WAIT_FOR_ADDRESS;
            break;
		default :
			break ;
    }
	return 1 ;
}


int main (void)
{
	int i = 0;
	uint8_t adc_chan = 0 ;
	init_led();
	i2cInit();
	i2cSetLocalDeviceAddr(TWI_SLA, 1);
	i2cSetSlaveReceiveHandler(evaluate_i2c_input);
	i2cSetSlaveTransmitHandler(read_callback);
	servoInit();
	for(i = 0 ; i < 6 ; i ++){
		servoSetChannelIO(i, _SFR_IO_ADDR(PORTB), i);
		DDRB |= (1 << i) ;
		servoSetPosition(i, 0x7F);
	}
	a2dInit();
	DDRC &=~ ((1 << PC0) | (1 << PC1) | (1 << PC2) | (1 << PC3) ); // all channels minus I2C
	PORTC &=~ ((1 << PC0) | (1 << PC1) | (1 << PC2) | (1 << PC3));
	a2dSetPrescaler(ADC_PRESCALE_DIV128);
	a2dSetReference(ADC_REFERENCE_256V);
	sei();
	i = 0 ;
	while(1)
	{
		if(commandReceived){
			if(command.state == WAIT_FOR_ADDRESS){
        		if(command.address < 6){
          			servoSetPosition(command.address, command.value);
        		}else if(command.address == IO_PORT_DDR){
					DDRD |= (command.value & IO_PORT_MASK) ;
				}else if(command.address == IO_PORT_VALUE){
					PORTD |= (command.value & IO_PORT_MASK) ;
				}
			}
			commandReceived = false ;
		}
		if(adc_chan < 4){
			adcs[adc_chan] = a2dConvert8bit(adc_chan);
		}else{
			adcs[adc_chan] = a2dConvert8bit(adc_chan+2);
		}
		adc_chan =  adc_chan + 1 ;
		if(adc_chan > 5){
		 	adc_chan = 0 ;
		}
	}
}


